/*
* @File Name collide.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\obstacle\collide.c
* @Author: Ruige_Lee
* @Date:   2019-08-19 20:14:17
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-08-29 20:18:33
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/

#include <stdint.h>

const float LinkRadius[4] = {80,70,60,60};

#include "fk/fK.h"
#include "math/fast_math.h"
#include "collide.h"
#include "stepper/steppers.h"





int32_t collide_check_top( float prmPose[6] )
{	
	int32_t status = 0;
	//获取障碍臂的姿态
	float obsPose[6];
	for ( uint8_t i = 0; i < 6; i++ )
	{
		obsPose[i] = stepper_getRad( 1, i ); //rad 格式
	}

	status = collide_pose_check( &prmPose[0], &obsPose[0] );
	
	return status;
}



/**
输入：两个机械臂的六维角度
输出：0 不碰撞
输出：1 碰撞
输出：-1 错误
*/
int32_t collide_pose_check(float prmPose[6], float obsPose[6])
{

	float LinkDot_A[5][3];
	float LinkDot_B[5][3];

	fK_nomal_solve(prmPose, LinkDot_A);
	fK_conversion_solve(obsPose, LinkDot_B);

	//self conllide checking
	

	//collide checking
	for ( uint8_t i = 0; i < 4; i ++ )
	{
		for ( uint8_t j = 0; j < 4; j++ )
		{
			//非同机械臂关节两两碰撞检测
			if ( 1 == LinkCollide( i, j, LinkDot_A, LinkDot_B) )
			{
				return 1;
			}
		}	
	}


	return 0;
}





/**
输入：两个linker的4个节点
输出：0 不碰撞
输出：1 碰撞
输出：-1 错误
*/
int32_t LinkCollide( uint8_t robLinkA, uint8_t robLinkB, float LinkDot_A[5][3], float LinkDot_B[5][3] )
{
	uint8_t i = robLinkA;
	uint8_t j = robLinkB;
	float mixPoint_A[3];
	float mixPoint_B[3];

	float distance;
	Linkdistance( LinkDot_A[i], LinkDot_A[i+1], LinkDot_B[j], LinkDot_B[j+1], mixPoint_A, mixPoint_B, &distance );

	if ( distance > ( ( LinkRadius[i] + LinkRadius[j] ) *  ( LinkRadius[i] + LinkRadius[j] ) ) )
	{
		return 0;
	}
	else
	{
		return 1;
	}
	
}




/**
 * 输入 两个linker直线的两点
 * 返回 两个垂足交点，（判定位置）
 * 返回 两线距离
 * 返回 10种可能状态
 */
int32_t Linkdistance( float LinkDot_A0[3], float LinkDot_A1[3], float LinkDot_B0[3], float LinkDot_B1[3], float mixPoint_A[3], float mixPoint_B[3], float *distance )
{
	int32_t res = 0;

	float Link_A_X0 = LinkDot_A0[0];
	float Link_A_X1 = LinkDot_A1[0];
	float Link_A_Y0 = LinkDot_A0[1];
	float Link_A_Y1 = LinkDot_A1[1];
	float Link_A_Z0 = LinkDot_A0[2];
	float Link_A_Z1 = LinkDot_A1[2];

	float Link_B_X0 = LinkDot_B0[0];
	float Link_B_X1 = LinkDot_B1[0];
	float Link_B_Y0 = LinkDot_B0[1];
	float Link_B_Y1 = LinkDot_B1[1];
	float Link_B_Z0 = LinkDot_B0[2];
	float Link_B_Z1 = LinkDot_B1[2];


	float ux = Link_A_X1 - Link_A_X0;
	float uy = Link_A_Y1 - Link_A_Y0;
	float uz = Link_A_Z1 - Link_A_Z0;

	float vx = Link_B_X1 - Link_B_X0;
	float vy = Link_B_Y1 - Link_B_Y0;
	float vz = Link_B_Z1 - Link_B_Z0;

	float wx = Link_A_X0 - Link_B_X0;
	float wy = Link_A_Y0 - Link_B_Y0;
	float wz = Link_A_Z0 - Link_B_Z0;

	float a = ( ux * ux + uy * uy + uz * uz ); //u*u
	float b = ( ux * vx + uy * vy + uz * vz ); //u*v
	float c = ( vx * vx + vy * vy + vz * vz ); //v*v
	float d = ( ux * wx + uy * wy + uz * wz ); //u*w 
	float e = ( vx * wx + vy * wy + vz * wz ); //v*w
	float dt = a * c - b * b;

	float sd = dt;
	float td = dt;

	float sn = 0.0;//sn = be-cd
	float tn = 0.0;//tn = ae-bd

	if ( IsEqual(dt, 0.0) == 1 )
	{
		sn = 0.0;    //在s上指定取s0
		sd = 1.00;   //防止计算时除0错误

		tn = e;      //按(公式3)求tc
		td = c;

		res = 1;//两直线平行
	}
	else//不平行
	{
		sn = (b * e - c * d);
		tn = (a * e - b * d);

		if ( sn < 0.0 )	//垂足在起点之外，最近点就是起点
		{
			//最近点在s起点以外，同平行条件
			sn = 0.0;

			tn = e;
			td = c;

			res |= 0x01;
		}
		else if ( sn > sd ) //垂足在终点之外，最近点就是终点
		{
			//最近点在s终点以外(即sc>1,则取sc=1)

			sn = sd;
			tn = e + b;
			td = c;

			res |= 0x02;
		}
		else // 垂足正好在机械臂关节上
		{
			res |= 0x00;
		}

	}

	if ( tn < 0.0 ) //垂足在起点之外，最近点为起点
	{
		//最近点在t起点以外
		tn = 0.0;

		
		if ( -d < 0.00 )
		{
			sn = 0.00;
		}
		else if ( -d > a )
		{
			sn = sd;
		}
		else
		{
			sn = -d;
			sd = a;
		}
		
		res |= 0x04;
	}
	else if ( tn > td ) //垂足在终点之外，最近点为终点
	{
		tn = td;
		
		if ( ( -d + b ) < 0.0 )
		{
			sn = 0.0;
		}
		else if ( ( -d + b ) > a )
		{
			sn = sd;
		}
		else
		{
			sn = ( -d + b );
			sd = a;
		}

		res |= 0x08;
	}
	else // 垂足正好在机械臂关节上
	{
		res |= 0x00;
	}

	
	



	float sc = 0.0;	//起点到垂足的长度(带正负)
	float tc = 0.0; //起点到垂足的长度(带正负)

	if ( IsEqual(sn, 0.0) )
	{
		sc = 0.0;
	}
	else
	{
		sc = sn / sd;
	}

	if ( IsEqual(tn, 0.0) )
	{
		tc = 0.0;
	}
	else
	{
		tc = tn / td;
	}

	float dx = wx + (sc * ux) - (tc * vx);
	float dy = wy + (sc * uy) - (tc * vy);
	float dz = wz + (sc * uz) - (tc * vz);

	

	mixPoint_A[0] = Link_A_X0 + sc * ux;
	mixPoint_A[1] = Link_A_Y0 + sc * uy;
	mixPoint_A[2] = Link_A_Z0 + sc * uz;


	mixPoint_B[0] = Link_B_X0 + tc * vx;
	mixPoint_B[1] = Link_B_Y0 + tc * vy;
	mixPoint_B[2] = Link_B_Z0 + tc * vz;


	*distance = dx * dx + dy * dy + dz * dz;

	return res;

}


